#include<Align.h>

Align::Align(Double_t vec1X, Double_t vec2X,Double_t vec1Y, Double_t vec2Y){
	  VecX_[0] = vec1X;
	  VecX_[1] = vec2X;

	  VecY_[0] = vec1Y;
	  VecY_[1] = vec2Y;
}

void Align::Aligment(Double_t APara[]){



  TMinuit* gMinuit = new TMinuit(6);
  
  gMinuit->SetFCN(fcn2);
  
  Double_t arglist[10];
  Int_t ierflg = 0;
    
  arglist[0] = 1;
  gMinuit->mnexcm("SET ERR", arglist ,1,ierflg);
  
  Double_t vstart[6] = {APara[0],APara[1],APara[2],APara[3],APara[4],APara[5]};
  Double_t step[6] = {0.1,0.1,0.1,0.00001,0.00001,0.0001};
  
  //void mnparm(		  Int_t 			k,				==>(external) parameter number
  //	  	  	  	  	  	  TString 		cnamj,		==>parameter name
  //	  	  	  	  	  	  Double_t 		uk,			==>starting value
  //	  	  	  	  	  	  Double_t 		wk,			==>starting step size or uncertainty
  //	  	  	  	  	  	  Double_t 		a,				==>lower physical parameter limits
  //	  	  	  	  	  	  Double_t 		b,				==>upper physical parameter limits
  //	  	  	  	  	  	  Int_t& 			ierflg)		==>Output:
  //	  	  	  	  	  	  	  	  	  	  	  	  	  	  	  	  	  IERFLG=0 if no problems
  //	  	  	  	  	  	  	  	  	  	  	  	  	  	  	  	  	  >0 if MNPARM unable to implement definition

  gMinuit->mnparm(0, "offsetX", vstart[0], step[0], 0,0,ierflg); 
  gMinuit->mnparm(1, "offsetY", vstart[1], step[1], 0,0,ierflg); 
  gMinuit->mnparm(2, "ofssetZ", vstart[2], step[2], 0,0,ierflg); 
  gMinuit->mnparm(3, "phiX", vstart[3], step[3], -22.0/7.0,22.0/7.0,ierflg); 
  gMinuit->mnparm(4, "phiY", vstart[4], step[4], -22.0/7.0,22.0/7.0,ierflg); 
  gMinuit->mnparm(5, "phiZ", vstart[5], step[5], -22.0/7.0,22.0/7.0,ierflg); 
  
  // Now ready for minimization step
  arglist[0] = 5000;
  arglist[1] = 1.;
  gMinuit->mnexcm("MIGRAD", arglist ,2,ierflg);
  
  // Print results
  Double_t amin,edm,errdef;
  Int_t nvpar,nparx,icstat;
  gMinuit->mnstat(amin,edm,errdef,nvpar,nparx,icstat);
  gMinuit->mnprin(3,amin);
  
  Double_t par0, err0;
  Double_t par1, err1;
  Double_t par2, err2;
  Double_t par3, err3;
  Double_t par4, err4;
  Double_t par5, err5;
  
  gMinuit->GetParameter(0,par0,err0);
  gMinuit->GetParameter(1,par1,err1);
  gMinuit->GetParameter(2,par2,err2);
  gMinuit->GetParameter(3,par3,err3);
  gMinuit->GetParameter(4,par4,err4);
  gMinuit->GetParameter(5,par5,err5);

  APara[0] = par0;
  APara[1] = par1;
  APara[2] = par2;
  APara[3] = par3;
  APara[4] = par4;
  APara[5] = par5;

  cout << " =>=>=>=>=>=> " << endl;
  printf("Xoffset=%f err=%f\n", par0, err0);
  printf("Yoffset=%f err=%f\n", par1, err1);
  printf("Zoffset=%f err=%f\n", par2, err2);
  printf("PhiX=%f err=%f \n", par3, err3);
  printf("PhiY=%f err=%f \n", par4, err3);
  printf("PhiZ=%f err=%f \n", par5, err3);

  printf("\n");
  printf("\n");
  printf("\n");

  /*
  Phi = TMath::ATan((TMath::Sin(par3)*TMath::Cos(par5) + TMath::Cos(par3)*TMath::Sin(par4)*TMath::Sin(par5))/(TMath::Sin(par3)*TMath::Sin(par5) - TMath::Cos(par3)*TMath::Sin(par4)*TMath::Cos(par5)));
  Theta = TMath::ACos(TMath::Cos(par3)*TMath::Cos(par4));

  cout << endl << " ********* PARAMETERS ********* " << endl << endl;
  cout << "  Phi = " << Phi * 630/11 << endl;
  cout << "  Theta = " << Theta * 630/11 << endl << endl;
  cout << " ********* PARAMETERS ********* " << endl << endl;
  */
  
}

void fcn2(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag){
	//npar 	: number of free parameters involved in minimization
	//gin 		: computed gradient values (optional)
	//f 		: the function value itself
	//par 		: vector of constant and variable parameters
	//flag 	: to switch between several actions of FCN

  Double_t chisq = 0.0;

  Double_t deltaX;
  Double_t deltaY;

  //for(Int_t i = 0; i < VecX[0]->size(); i++){

    deltaX = VecX_[0] - TransX( VecX_[1], VecY_[1], par);
    deltaY = VecY_[0] - TransY( VecX_[1], VecY_[1], par);

    chisq += deltaX*deltaX + deltaY*deltaY;

  //}//i
  
  f = chisq;

}//fcn


Align::~Align(){

}
